/*
 *  truss.cpp
 *  EPPI-0.0
 *
 *  Created by Juan Carlos Garcia Orden on 15/11/07.
 *
 */

#include "truss.h"

Truss::Truss()
:	Component(),
	dis(1.0),
	stretch(1.0),
	force(0.0),
	initialDis(0.0),
	mass(0.0),
	CConv(),
	areaIni(1.0),
	toldenom(1.0e-12),
	material()
{}

// ***

Truss::Truss(vector<string>& vp, double a, string& mn, int di)
:	dis(0.0),
	stretch(1.0),
	stretchC(1.0),
	WConv(0.0),
	mass(1.0),
	force(0.0),
	areaIni(a),
	toldenom(1.0e-12),
	material()
{
	C.setZero(6);
}

// ***

void Truss::calcFi(const vector<double>& qddot)
{
	int i;
	int j;
	
	for(i=0; i<numDofs; ++i)
	{
		Fi[i] = 0.0;
		for(j=0; j<numDofs; ++j)	
		{
			if(integratorType==0)  Fi[i] += M(i,j) *qddot[ localGlob[j] ];
			else	Fi[i] += M(i,j) * (dq[j] - dqC[j]);
		}
	}
	//cout << "Fi es " << endl;
}

// ***

void Truss::calcFq(const int& nt)
{
	double st;
	
	calcDisAndDir();
	
	if(initialDis>0.0)
	{
		stretch = dis / initialDis;	// stretch = dis/initialDis
		
		if(integratorType!=2)		// Non-conservative
		{
		  C(0) = stretch * stretch;
		  material->calcS(C);
		  force = areaIni * stretch * material->getS()[0] ; // force = sigma*currentArea
		}
		else	// Conservative
		{
		  st = (Q.tail(dim) - Q.head(dim)).norm() / initialDis;
		  C(0) = st * st;
		  
		  if (fabs(st - stretchC) < toldenom)
//		  if ( st == stC )
		  {
			material->calcS(C);
		    force = areaIni * st * material->getS()[0]; // sigma0 = force / (st * initialDis);
		  }
		  else
		  {
			material->calcW(C);
		    force = 2.0 * areaIni * ( material->getW() - WConv ) * stretch / (st * st - stretchC * stretchC);
		  }
		}
	}
	else 
	{
		force = 0.0;
	}
	
	Fq.head(dim) = - force * u;
	Fq.tail(dim) = - Fq.head(dim);
	
// 	cout << "Fq = " << endl;
// 	printEigenVectorXd(Fq);
}

// ***

void Truss::calcFt(const double& t, const double& grav, const int& gravDir)
{}

// ***

void Truss::calcKi(double & partial_qddot)
{
	if(partial_qddot!=partial_qddotOld)	// In order to compute Ki just once
	{
//		int i;
//		int j;
//		for(i=0; i<numDofs; i++)
//			for(j=0; j<numDofs; j++) Ki[i][j] = M(i,j)*partial_qddot;
		Ki = M * partial_qddot;
	}	
	partial_qddotOld = partial_qddot;
}

// ***

void Truss::calcKq(int nt)
{
	int i, j;
	double st;
	
	if(dis>0.0)
	{
		if (integratorType!=2) //means standard integrator and midpoint
		{
		    // Material
			Kq.block(0,0,dim,dim) = areaIni * material->getDDW(C) / initialDis * u * u.transpose();
			
			// Geometric
			Kq.block(0,0,dim,dim) += force/(stretch*initialDis) * ( MatrixXd::Identity(dim,dim) - u * u.transpose() );
			
			if(integratorType==1) // midpoint
			{
			    Kq *= 0.5;
			}
		}
	        else //means conservative
		{	
			st = (Q.tail(dim) - Q.head(dim)).norm() / initialDis;
			uq = (Q.tail(dim) - Q.head(dim)) / (st*initialDis);
			
			// Geometric
			double coefGeom;
			
			if (fabs(st-stretchC) < toldenom)	coefGeom = force/(st*initialDis);
//			if ( st == stretchC )	coefGeom = force/(stC*initialDis);
			else coefGeom = force/(stretch * initialDis);
			
			// Material
			double coefMat;
//			if (st == stC)
			if (fabs(st - stretchC) < toldenom)
			{
			        coefMat = (areaIni / initialDis * material->getDDW(CConv) - coefGeom)/2.0;
				
				Kq.block(0,0,dim,dim) = coefMat * u * u.transpose();
			}
			else
			{
				material->calcS(C);
				coefMat = 2.0 * ( areaIni / initialDis * st * material->getS()[0] - coefGeom * st) * stretch / (st * st - stretchC * stretchC);
				
				Kq.block(0,0,dim,dim) = coefMat * u * uq.transpose();
			}
			
			Kq.block(0,0,dim,dim) += 0.5 * coefGeom * MatrixXd::Identity(dim,dim);
		}
		// Finish to fill the matrix up
		
		Kq.block(0,dim,dim,dim) = - Kq.block(0,0,dim,dim);
		Kq.block(dim,0,dim,dim) = - Kq.block(0,0,dim,dim);
		Kq.block(dim,dim,dim,dim) = Kq.block(0,0,dim,dim);
	}
// 	cout << "Kq = " << endl;
// 	printEigenMatrixXd(Kq);
}	

// ***

void Truss::calcKdq(double & partial_qdot)
{}

// ***

void Truss::calcM(const vector<double>& q, const double& t)
{
	int i;
	if(M.size()==0)
	{	
		M.setZero(numDofs,numDofs);
	}
	// Consistent mass matrix
	if(numDofs==6)	// dim=3
	{
		M(0,0) = mass/3.;
		M(0,3) = mass/6.; 
		M(1,1) = mass/3.; 
		M(1,4) = mass/6.;
		M(2,2) = mass/3.;
		M(2,5) = mass/6.;
		M(3,0) = M(0,3);
		M(3,3) = mass/3.;
		M(4,1) = M(1,4); 
		M(4,4) = mass/3.;
		M(5,2) = M(2,5);
		M(5,5) = mass/3.;	
	} 
	else if(numDofs==4)	// dim=2
	{
		M(0,0) = mass/3.;
		M(0,2) = mass/6.;
		M(1,1) = mass/3.;
		M(1,3) = mass/6.;
		M(2,0) = M(0,2);
		M(2,2) = mass/3.;
		M(3,1) = M(1,3);
		M(3,3) = mass/3.;
	} 
	else			// dim=1
	{
		M(0,0) = mass/3.;
		M(0,1) = mass/6.;
		M(1,0) = M(0,1);
		M(1,1) = mass/3.;
	}
// 	printEigenMatrixXd(M);
}

// ***

void Truss::calcMLumped(const vector<double>& q, const double& t)
{
	if (MLumped.size()==0)
	{
		MLumped.resize(numDofs);
	}
	
	int i;
	for (i=0; i<numDofs; ++i)	MLumped[i] = mass/2.0;
}

// ***

void Truss::setDofs(void)
{
	map<string,Point*,less<string> >::iterator i;
	for(i=mPoints.begin(); i!=mPoints.end(); i++)	/** Loop on points belonging to this component */
	{
		for(int j=0; j<dim; j++)	(*i).second->setDof(0,j);	// Only translational dof */
		for(int j=0; j<dim; j++)	(*i).second->setDofInertia(0,j);
	}
	numDofs=dim*pointNames.size();
	numDofsInertia = numDofs;
}

// ***

void Truss::initializeGlobalDofs(vector<double> &q_in, vector<double> &dq_in, const double & t)
{
	int i;
	int j;
	int p;
	
	// Initial position
	q.resize(numDofs);
	Q.setZero(numDofs);
	u.setZero(dim);
	
	i=0;
	for(p=0; p<pointNames.size(); ++p)
	{
		for(j=0; j<dim; j++)
		{
			// Default: initial position dofs are point coordinates
			// they go to the firsts positions
			q_in[ localGlob[i*dim+j] ] = mPoints[pointNames[p]]->r[j];
		}
		i++;
	}
	
	// WARNING:
	// Remember that the ordering of vector pointNames and map mPoints is different
	initialDis = lTwoNorm(mPoints[pointNames[0]]->r, mPoints[pointNames[1]]->r);
	
	// Initial velocity
	// Default: initial velocity dofs are null
	dq.resize(numDofs);
	
	mass = initialDis*areaIni*(material->getDensity());
	
}

// ***

void Truss::setMatAtComp(Material* ma)
{
	material = ma;
}

// ***

void Truss::initializeCompRes(const bool& st, const double& grav, const int& gravDir, const bool augLagFlag)
{
	Fq.setZero(numDofs);
	
	if(grav!=0.0)
	{
		int i;
		int j;		
		Ft.resize(numDofs);
		// Build Ft once for all  Ft= mass * g
		Ft[gravDir-1] = - mass * grav / 2.0;
		Ft[dim+gravDir-1] = - mass * grav / 2.0;
	}
	
	if(st!=0)
	{
	  Fi.resize(numDofs);
	  //M.setZero(numDofs,numDofs);
	}
}

// ***

//void Truss::initializeCompTang(const bool& st,  const bool& lmf)
void Truss::initializeCompTang(const bool& st)
{
	int i;

	Kq.setZero(numDofs,numDofs);
	
	if(st!=0)
	{		
//		Ki.resize(numDofs);
//		for(i=0; i<numDofs;++i)	Ki[i].resize(numDofs);
		Ki.setZero(numDofs,numDofs);
		/*
		if (lmf==false)	// Consistent mass matrix
		{
			if(M.size()==0)	// Just in case M has not been created in initializeCompRes
			{	
				M.resize(numDofs);
				for(i=0; i<numDofs;++i)	M[i].resize(numDofs);
				makeM();	// Mass matrix can be defined once for all, since it is constant
			}
		}
		*/
		/*
		else	// Lumped mass matrix
		{
			if (MLumped.size()==0)
			{
				MLumped.resize(numDofs);
				makeMLumped();
			}
		}
		*/
//		Kdq.resize(numDofs);
//		for(i=0; i<numDofs;++i)	Kdq[i].resize(numDofs);
	}	
}
// ***

void Truss::calcDisAndDir()
{
  int i;
  if(integratorType==0)  // means standar integrator;
  {
    for(i=0; i<numDofs; ++i) Q[i] = q[i];

    dis = (Q.tail(dim) - Q.head(dim)).norm();
    
    if(dis>0.0)
    {
      u = (Q.tail(dim) - Q.head(dim)) / dis;
    }
  }
  else //means midpoint or conservative
  {
    for(i=0; i<numDofs; ++i)
    {
      Q[i] = q[i];
      QConv[i] = qC[i];
    }
      QMid = 0.5 * (Q + QConv);
      
      dis = (QMid.tail(dim) - QMid.head(dim)).norm();
      
      if(dis>0.0)	  u = (QMid.tail(dim) - QMid.head(dim)) / dis;
  }
}

// ***

void Truss::calcEner(double& Ekin, double& Epot, double& Econ, double& Eext, double& Eamb, double& Dint, double& grav, int& gravDir, vector<double>& q_in, vector<double>& dq_in)
{
	int i;
	// Kinetic
	if(dq.size()!=0 && (M.size()!=0 || MLumped.size()!=0) )
	{
		int j;
		for (i=0; i<numDofs; ++i)	dq[i] = dq_in[localGlob[i]];
		for (i=0; i<numDofs; ++i)
		{
			if (M.size()!=0)	// Consistent
			{
				for (j=0; j<numDofs; ++j)
				{
					Ekin+= 0.5 * dq[i] * M(i,j) * dq[j];
				}
			}
			else	// Lumped
			{
				Ekin+= 0.5 * dq[i] * MLumped[i] * dq[i];
			}						
		}
	}
	// Potential
	//for (i=0; i<numDofs; ++i)	q[i] = q_in[localGlob[i]];
	
	for (i=0; i<numDofs; ++i)
	{
		q[i] = q_in[localGlob[i]];
	}
	
	// Potential
	// st = (Q - fixedP).norm() / initialDis;
	
	stretch = lTwoNorm(q,dim);
	
	//dis = (Q.tail(dim) - Q.head(dim)).norm();
	stretch /= initialDis;
	
	C(0) = stretch * stretch;
	material->calcW(C);
	Epot += material->getW() * areaIni * initialDis;
	if (grav != 0.0) Epot += mass * grav * (q[gravDir - 1] + q[gravDir - 1 + dim]) / 2.0;  
}

// ***

void Truss::calcEntropy(double& Entropy, double& EntropyAmb)
{}

// ***

void Truss::calcAM(vector<double>& am, vector<double>& q_in, vector<double>& dq_in)
{}	

// ***

void Truss::updateComp(void)
{
    if(integratorType==1)
    {
      if(QConv.size()==0)	QConv.setZero(numDofs);
    }
    if(integratorType==2)	
    {
      if(QConv.size()==0)	QConv.setZero(numDofs);
      if(QMid.size()==0)	QMid.setZero(numDofs);
      if(uq.size()==0)		uq.setZero(dim);
      if(CConv.size()==0)	CConv.setZero(6);
      
      calcDisAndDir();
      stretchC = (QConv.tail(dim) - QConv.head(dim)).norm() / initialDis;
      CConv(0) = stretchC * stretchC;
      material->calcW(CConv);
      WConv = material->getW();
    }
}

// ***

void Truss::setIniDofs(vector<double>&)
{}

// ***

void Truss::calcPhi(const vector<double>&)
{}

// ***

void Truss::calcDPhi(const vector<double>&)
{}

// ***

void Truss::calcMSmooth(const vector<double>& q_in, const double& t)
{
	int i;
	int j;
	
	MSmooth.setZero(numDofsInertia, numDofsInertia);

//	if(MSmooth.size()==0)	// Just in case M has not been created
//	{
//		MSmooth.resize(numDofsInertia);
//		for(i=0; i<numDofsInertia;++i)	MSmooth[i].resize(numDofsInertia);
//	}
	
	if (M.size()==0)	calcM(q_in,t);
	
	if (material->getDensity()!=0.0)
	{
//		for(i=0; i < numDofs; i++)
//		{
//			for(j=0; j < numDofs; j++)
//			{
//				MSmooth(i,j) = M(i,j)/(material->getDensity());
//			}
//		}
		MSmooth = M / (material->getDensity());
	}
	else
	{
//	  	for(i=0; i < numDofs; i++)
//		{
//			for(j=0; j < numDofs; j++)
//			{
//				MSmooth(i,j) = M(i,j);
//			}
//		}
	  	MSmooth = M;
	}
	/*
	int i;
	if(MSmooth.size()==0)
	{	
		MSmooth.resize(numDofsInertia);
		for( i=0;i<numDofsInertia;i++) MSmooth[i].resize(numDofsInertia);
	}
	for(i=0; i<numDofsInertia;i++) MSmooth[i][i] = 1.0;	
	*/
}

// ***

void Truss::calcFSmooth(const vector<double>& q_in, const vector<double>& dq_in, const double& t)
{
	int i;
	if (FSmooth.size()==0)		// Just in case M has not been created in initializeCompRes
	{
		FSmooth.setZero(2,numDofsInertia);
	//	FSmooth.resize(2);
	//	for (i=0; i<2; ++i)	FSmooth[i].resize(numDofsInertia);
	}
	//else setZero(FSmooth);
}

int Truss::checkState()
{
return 0;
}
